/*
 *  Copyright (C) 2021 张伊男
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include "RuneDetector.h"

RuneDetector::RuneDetector()
{
    
    mutex=small_rune;
    judge_shunni=false;
    method=use_R;
    least_armor_num=10;
    sample_time=0.025; 
    small_detect_angle=25;
    big_detect_angle=20;
    
    for(int i=1;i<=8;i++)
    {
        templ[i]=imread("../Template-Rune/template"+to_string(i)+".jpg",IMREAD_GRAYSCALE);
        threshold(templ[i], templ[i], 100, 255, THRESH_BINARY_INV);
        resize(templ[i],templ[i],Size(40,20), 0, 0, INTER_LINEAR);
    }
}

RuneDetector::~RuneDetector()
{
}

bool RuneDetector::DetectRune(cv::Mat& src, cv::Point3f& target)
{
    if(mcu_data.state == State::RUNE_STATE) mutex=small_rune;
    else if(mcu_data.state == State::RUNE_BIG) mutex=big_rune;
    else cout<<"rune mutex error!"<<endl;

    double t=(double)cv::getTickCount();

    Mat dst1;   
    dst1=pretreatment(src);
    bool isnot=get_armor(dst1);
    //cout<<endl<<endl;
    if(isnot)   next_armor=detect_next();
    else 
    {
	//cout<<"no armor!"<<endl<<endl;
	//waitKey(0);
	return false;
    }
    
    cal_target(target);

    t=((double)cv::getTickCount()-t)/cv::getTickFrequency();
    sample_time=t;
    //cout<<int(1.0/t)<<" ";
#ifdef debug_img   
    paint(src,dst1,isnot,target,int(1.0/t));
#endif
    return true;
}

Mat RuneDetector::pretreatment(Mat src)
{
    Mat gray_src,dst,element;
    vector<Mat> channels;
    split(src,channels);
    
    if(mcu_data.enemy_color == EnemyColor::BLUE)
    	gray_src=channels[2]-channels[0];
    else gray_src=channels[0]-channels[2];

    //imshow("xiangjian",gray_src);

    if(mcu_data.enemy_color == EnemyColor::BLUE)
        threshold(gray_src,dst,runeParam.red_thresh,255,THRESH_BINARY);//阈值处理得到二值图
    else threshold(gray_src,dst,runeParam.blue_thresh,255,THRESH_BINARY);
    //imshow("erzhi",dst);
    element = getStructuringElement(MORPH_ELLIPSE, Size(3, 3)); //膨胀处理，使得图像中的轮廓更加明显(3,3!!)
    dilate(dst, dst, element);
    //imshow("pengzhang",dst);
    //开运算，消除扇叶上可能存在的小洞
    int structElementSize=5;//4不够
    element=getStructuringElement(MORPH_RECT,Size(2*structElementSize+1,2*structElementSize+1),Point(structElementSize,structElementSize));
    morphologyEx(dst,dst, MORPH_CLOSE, element);

    return dst;
}

//获取点间距离
double getDistance(Point A,Point B)
{
    double dis;
    dis=pow((A.x-B.x),2)+pow((A.y-B.y),2);
    return sqrt(dis);
}

bool RuneDetector::get_armor(Mat &src)
{
    vector<vector<Point>> contours;
    vector<Vec4i> hierarchy;
    findContours(src,contours,hierarchy,CV_RETR_TREE,CHAIN_APPROX_SIMPLE);
    RotatedRect rect;
    vector<double> area_num;
    vector<int> index_num;

    R_impossible.clear();

    //遍历轮廓
    if(hierarchy.size())
        for(int i=0;i>=0;i=hierarchy[i][0])
        {
            rect=minAreaRect(contours[i]);
            Point2f P[4];
            rect.points(P);
            Point2f srcRect[4],dstRect[4];
            double width,height;
            //矫正提取的叶片的宽高
            width=getDistance(P[0],P[1]);
            height=getDistance(P[1],P[2]);
            if(width>height)
            {
                srcRect[0]=P[0];
                srcRect[1]=P[1];
                srcRect[2]=P[2];
                srcRect[3]=P[3];
            }
            else
            {
                swap(width,height);
                srcRect[0]=P[1];
                srcRect[1]=P[2];
                srcRect[2]=P[3];
                srcRect[3]=P[0];
            }
            //通过面积筛选
            double area=height*width;
#ifdef debug_console
            cout<<"area_windmill:"<<area<<"  ";
#endif
            if(area<runeParam.shanye_min_area) 
	    {
		R_impossible.push_back(rect);		
		continue;
	    }
            else
            {
                dstRect[0]=Point2f(0,0);
                dstRect[1]=Point2f(width,0);
                dstRect[2]=Point2f(width,height);
                dstRect[3]=Point2f(0,height);
                // 应用透视变换，矫正成规则矩形
                Mat transform = getPerspectiveTransform(srcRect,dstRect);
                Mat perspectMat;
                warpPerspective(src,perspectMat,transform,src.size());
                // 提取扇叶图片
                Mat testim;
                testim = perspectMat(Rect(0,0,width,height));
#ifdef use_template
                if(!match_template(testim)) continue;
#endif
#ifdef use_svm
                if(!svm(testim)) continue;
#endif
#ifdef use_area
                if(width/height<2||width/height>2.5) continue;
#endif
                else
                {
#ifdef debug_console
                    cout<<"hierarchy[i][2]:"<<hierarchy[i][2]<<" ";
#endif
		    area_num.push_back(area);
		    index_num.push_back(i);
                    
                 }
              }
          }

    //查找装甲板
    if(index_num.size()>0)
    {
    int min_index=0;
    for(int j=0;j<index_num.size();j++)
    {
	if(area_num[j]<area_num[min_index]) min_index=j;
	else ;
    }
    int i=index_num[min_index];
    if(hierarchy[i][2]>=0)
    {
        RotatedRect rect_tmp=minAreaRect(contours[hierarchy[i][2]]);
        Point2f Pnt[4];
        rect_tmp.points(Pnt);

        float width=rect_tmp.size.width;
        float height=rect_tmp.size.height;
        if(height>width) swap(height,width);//height与width谁大谁小不定（这里也可max,min）
        float area=width*height;
#ifdef debug_console
        cout<<"area_armor:"<<area<<"  ";
        cout<<"height/width:"<<height/width<<"  ";
#endif
        if(height/width>runeParam.maxHWRatio||height/width<runeParam.minHWRatio||area>runeParam.maxArea ||area<runeParam.minArea) return false;
        else
        {
            armor_rect=rect_tmp;
            return true;
        }
     }
     }

     else return false;
}

RotatedRect RuneDetector::detect_next()
{
    Point2f cc;//风车圆心
    Point2f centerP=armor_rect.center;//当前帧装甲板中心点（待拟合的数据点）
    float R;//风车半径-0
    float big_now_speed;//大风车需要计算当前速度

    if(method == use_R)
    {
        bool is_get_R=get_R_icon(cc,R);
#ifdef debug_console
        if(is_get_R) ;//cout<<"1"<<endl
        else cout<<"0"<<endl;
#endif
        if(!is_get_R)//1.由于枪口移动角度过大导致看不到R标  2.掉帧导致找不到R，就会利用拟合圆方式
            //get_fit_circle(cc,R);
        {
            /*RotatedRect rect;
	    for(int i=0;i<R_impossible.size();i++)
            {
	    rect=R_impossible[i];

            cout<<rect.size.area()<<endl;
            cout<<rect.size.height/rect.size.width<<endl;
            cout<<rect.size.area()/armor_rect.size.area()<<endl<<endl;
	    }
	    waitKey();*/
	    RotatedRect resrect;
	    resrect.center=Point2f(0,0);
	    return resrect;
        }    
    }//找不到R标是使用拟合圆还是放弃该帧
    else if(method == fit_circle)
    {
        get_fit_circle(cc,R);
    }
    else
    {
        cout<<"detect_method类无此变量！！"<<endl;
        exit(0);
    }

    if(!judge_shunni)
    {
        if(angle_num.size()<20)
        {
            float now_angle;//=acos((armor_rect.center.x-cc.x)/R)不可，确定角度需要两个方程联立（这里利用一个方程加象限）
	    float tmp=acos(abs(armor_rect.center.x-cc.x)/R)*57.32;
            if(armor_rect.center.x>=cc.x&&armor_rect.center.y<=cc.y)
	    	now_angle=tmp;
            else if(armor_rect.center.x<cc.x&&armor_rect.center.y<cc.y)
	    	now_angle=180-tmp;
            else if(armor_rect.center.x<cc.x&&armor_rect.center.y>cc.y)
	    	now_angle=180+tmp;
            else if(armor_rect.center.x>cc.x&&armor_rect.center.y>cc.y)
	    	now_angle=360-tmp;
           
            angle_num.push_back(now_angle);
            RotatedRect resrect;
            resrect.center=Point2f(0,0);
            return resrect;
        }
        else
        {
            int score1,score2;//顺+逆
            score1=score2=0;
            for(int i=0;i<angle_num.size()-1;i++)
            {
	  if(angle_num[i]>angle_num[i+1]) score1++;
	  else score2++;
            }
            if(score1>score2)//顺
            {
                 small_detect_angle=abs(small_detect_angle);
                 big_detect_angle=abs(big_detect_angle);
            }
            else
            {
                 small_detect_angle=-1*abs(small_detect_angle);
                 big_detect_angle=-1*abs(big_detect_angle);
            }
            judge_shunni=true;
            angle_num.clear();
	    //angle_num.resize(0);
        }
    }
    else ;

    if(mutex==small_rune)
    {                   
	    Mat rot_mat=getRotationMatrix2D(cc,small_detect_angle,1);
	    float sinA=rot_mat.at<double>(0,1);
	    float cosA=rot_mat.at<double>(0,0);
	    float xx=-(cc.x-centerP.x);
	    float yy=-(cc.y-centerP.y);
	    Point2f resPoint=Point2f(cc.x+cosA*xx-sinA*yy,cc.y+sinA*xx+cosA*yy);

	    RotatedRect resrect;
             	    resrect.center=resPoint;
        	    resrect.angle=armor_rect.angle+small_detect_angle;
        	    resrect.size.height=armor_rect.size.height;
        	    resrect.size.width=armor_rect.size.width;

	    //resrect.center.y=(resrect.center.y-2.5*resrect.size.height)>0?(resrect.center.y-2*resrect.size.height):(1.5*resrect.size.height);

	    return resrect;
    }
    else if(mutex==big_rune)
    {
        float now_angle;
	float tmp=acos(abs(armor_rect.center.x-cc.x)/R);//弧度
	if(armor_rect.center.x>=cc.x&&armor_rect.center.y<=cc.y)
    	    now_angle=tmp;
        else if(armor_rect.center.x<cc.x&&armor_rect.center.y<cc.y)
    	    now_angle=3.14-tmp;
        else if(armor_rect.center.x<cc.x&&armor_rect.center.y>cc.y)
    	    now_angle=3.14+tmp;
    	else if(armor_rect.center.x>cc.x&&armor_rect.center.y>cc.y)
    	    now_angle=6.28-tmp;
        if(angle_num.size()<2)
	{           	    
	    angle_num.push_back(now_angle);
	    RotatedRect resrect;
            resrect.center=Point2f(0,0);
            return resrect;
	}
	else ;

	big_now_speed=abs(angle_num[0]-angle_num[1])/sample_time;//弧度
	angle_num.erase(angle_num.begin());
	angle_num.push_back(now_angle);
	//cout<<"w(角度/秒)"<<abs(angle_num[0]-angle_num[1])/0.025<<endl;两帧相距0.025秒
	//cout<<"w(rad/s):"<<big_now_speed<<endl;
        float t,spd[20];
        t=asin((abs(big_now_speed)-1.305)/0.785)/1.884;
	//cout<<"now time:"<<t<<endl;
        float angle_variation=0;
	float variation_time=runeParam.delay_time/20;//延迟delay_time秒，分20份
        if(t>=0&&t<=3.33)
        {
        	for(int i=0;i<20;i++,t+=variation_time)
        	{
		    spd[i]=0.785*sin(1.884*t)+1.305;
		    angle_variation+=spd[i]*variation_time;
        	}
		angle_variation*=big_detect_angle>0?1:-1;
        }
        else angle_variation=big_detect_angle;
        
	angle_variation=angle_variation*57.29578;//弧度转角度
	cout<<"angle_variation:"<<angle_variation<<endl;
        float sinA=sin(angle_variation);
        float cosA=cos(angle_variation);
        float xx=-(cc.x-centerP.x);
        float yy=-(cc.y-centerP.y);
        Point2f resPoint=Point2f(cc.x+cosA*xx-sinA*yy,cc.y+sinA*xx+cosA*yy);

        RotatedRect resrect;
        resrect.center=resPoint;
        resrect.angle=armor_rect.angle+angle_variation;
        resrect.size.height=armor_rect.size.height;
        resrect.size.width=armor_rect.size.width;

	//resrect.center.y=(resrect.center.y-2.5*resrect.size.height)>0?(resrect.center.y-2*resrect.size.height):(1.5*resrect.size.height);

        return resrect;
    }
    else
    {
        cout<<"风车能量机关模式设置错误：detext函数中mutex没有这个选项"<<endl;
        exit(0);
    }
}

double TemplateMatch(cv::Mat image, cv::Mat tepl, cv::Point &point, int method)
{
    int result_cols =  image.cols - tepl.cols + 1;
    int result_rows = image.rows - tepl.rows + 1;
    cv::Mat result = cv::Mat( result_cols, result_rows, CV_32FC1 );
    cv::matchTemplate( image, tepl, result, method );

    double minVal, maxVal;
    cv::Point minLoc, maxLoc;
    cv::minMaxLoc( result, &minVal, &maxVal, &minLoc, &maxLoc, Mat() );

    switch(method)
    {
    case CV_TM_SQDIFF:
    case CV_TM_SQDIFF_NORMED:
        point = minLoc;
        return minVal;

    default:
        point = maxLoc;
        return maxVal;

    }
}

bool RuneDetector::match_template(Mat &testim)
{
    Mat tmp[9];

    Point matchLoc;
    double value;
    Mat tmp1;
    resize(testim,tmp1,Size(40,20), 0, 0, INTER_LINEAR);

    vector<double> Vvalue1;
    vector<double> Vvalue2;
    for(int j=1;j<=6;j++)
    {
        value = TemplateMatch(tmp1, templ[j], matchLoc, CV_TM_CCOEFF_NORMED);
        Vvalue1.push_back(value);
    }
    for(int j=7;j<=8;j++)
    {
        value = TemplateMatch(tmp1, templ[j], matchLoc, CV_TM_CCOEFF_NORMED);
        Vvalue2.push_back(value);
    }
    int maxv1=0,maxv2=0;
    for(int t1=0;t1<6;t1++)
    {
        if(Vvalue1[t1]>Vvalue1[maxv1])
        {
            maxv1=t1;
        }
    }
    for(int t2=0;t2<2;t2++)
    {
        if(Vvalue2[t2]>Vvalue2[maxv2])
        {
            maxv2=t2;
        }
    }
#ifdef debug_console
    cout<<"Vvalue1[maxv1]:"<<Vvalue1[maxv1]<<"   "<<"Vvalue2[maxv2]:"<<Vvalue2[maxv2]<<"  "<<endl;
#endif
    if(Vvalue1[maxv1]>Vvalue2[maxv2]&&Vvalue1[maxv1]>runeParam.match_limit) return true;
    else return false;
}

bool CircleInfo2(vector<RotatedRect>& pts, Point2f& center, float& radius)
{
    center = Point2d(0, 0);
    radius = 0.0;//初始化接收到的参数

    double sumX = 0.0;
    double sumY = 0.0;
    double sumX2 = 0.0;
    double sumY2 = 0.0;
    double sumX3 = 0.0;
    double sumY3 = 0.0;
    double sumXY = 0.0;
    double sumX1Y2 = 0.0;
    double sumX2Y1 = 0.0;
    const double N = (double)pts.size();
    for (int i = 0; i < pts.size(); ++i)
    {
        double x = pts[i].center.x;
        double y = pts.at(i).center.y;
        double x2 = x * x;
        double y2 = y * y;
        double x3 = x2 *x;
        double y3 = y2 *y;
        double xy = x * y;
        double x1y2 = x * y2;
        double x2y1 = x2 * y;

        sumX += x;
        sumY += y;
        sumX2 += x2;
        sumY2 += y2;
        sumX3 += x3;
        sumY3 += y3;
        sumXY += xy;
        sumX1Y2 += x1y2;
        sumX2Y1 += x2y1;
    }
    double C = N * sumX2 - sumX * sumX;
    double D = N * sumXY - sumX * sumY;
    double E = N * sumX3 + N * sumX1Y2 - (sumX2 + sumY2) * sumX;
    double G = N * sumY2 - sumY * sumY;
    double H = N * sumX2Y1 + N * sumY3 - (sumX2 + sumY2) * sumY;

    double denominator = C * G - D * D;
    if (abs(denominator) < DBL_EPSILON) return false;//值得学习（分母不是和0比，而是DBL_EPSILON+0）
    double a = (H * D - E * G) / (denominator);
    denominator = D * D - G * C;
    if (abs(denominator) < DBL_EPSILON) return false;
    double b = (H * C - E * D) / (denominator);
    double c = -(a * sumX + b * sumY + sumX2 + sumY2) / N;

    center.x = a / (-2);
    center.y = b / (-2);
    radius = sqrt(a * a + b * b - 4 * c) / 2;
    return true;
}

void RuneDetector::get_fit_circle(Point2f &cc,float &R)
{
    if(armor_num.size()<least_armor_num)//30?
    {
        armor_num.push_back(armor_rect);
    }
    else
    {
        CircleInfo2(armor_num,cc,R);
#ifdef debug_console
        cout<<"拟合圆心:"<<cc.x<<" "<<cc.y<<" 拟合半径："<<R<<endl;
#endif
    armor_num.erase(armor_num.begin());//更新
    armor_num.push_back(armor_rect);        
    }
    return ;
}

bool RuneDetector::get_R_icon(Point2f &center,float &R)
{
        RotatedRect rect;
	for(int i=0;i<R_impossible.size();i++)
        {
	    rect=R_impossible[i];
#ifdef debug_console
            cout<<rect.size.area()<<endl;
            cout<<rect.size.height/rect.size.width<<endl;
            cout<<rect.size.area()/armor_rect.size.area()<<endl<<endl;
#endif
            if(rect.size.area()>runeParam.R_max_area||rect.size.area()<runeParam.R_min_area) continue;
            if(abs(rect.size.height/rect.size.width-1)>0.3) continue;
            if(rect.size.area()/armor_rect.size.area()>runeParam.R_max_ratio||rect.size.area()/armor_rect.size.area()<runeParam.R_min_ratio) continue;
            //if(90+rect.angle>10) continue;R标会转

            center=rect.center;
            R=sqrt(pow(rect.center.x-armor_rect.center.x,2)+pow(rect.center.y-armor_rect.center.y,2));
#ifdef debug_console
            cout<<"利用图标圆心:"<<rect.center.x<<" "<<rect.center.y<<" 利用图标半径："<<R<<endl;
#endif
	    my_R=rect;
	    is_get_R=true;
            return true;
        }
    is_get_R=false;
    return false;
}

void RuneDetector::cal_target(Point3f& target)
{
	RotatedRect next_armor1;
	if(next_armor.center!=Point2f(0,0))
	{
	    next_armor1.center=next_armor.center;
	    next_armor1.angle=0;
	    next_armor1.size.height=next_armor.size.height;
	    next_armor1.size.width=next_armor.size.width;
	}
	else
	{
	    next_armor1.center=armor_rect.center;
	    next_armor1.angle=0;
	    next_armor1.size.height=armor_rect.size.height;
	    next_armor1.size.width=armor_rect.size.width;
	}
	       

	static float half_camera_width = 320;
        static float half_camera_height = 280;
        static float camera_fx = 1289.1;//cameraParam.fx
        static float camera_fy = 1228.7;//cameraParam.fy
        constexpr double rad = 57.295779513082320876798154814105;

        Point2f armor[4];
        next_armor1.points(armor);

        Point2f armor_up = (armor[0] + armor[1]) / 2;
        Point2f armor_down = (armor[2] + armor[3]) / 2;
        float armor_height = sqrt(powf(armor_down.x - armor_up.x, 2) + powf(armor_down.y - armor_up.y, 2));
        Point2f armor_center = (armor_up + armor_down) / 2;

        float yaw_offset = atan2(half_camera_width - armor_center.x, camera_fx) * rad;
        float pitch_offset = atan2(half_camera_height - armor_center.y, camera_fy) * rad;
        float distance = camera_fx * 125 / armor_height / 1000;
#ifdef debug_console
        cout << "\nyaw=" << yaw_offset;
        cout << "\npitch=" << pitch_offset;
        cout << "\ndistance" << distance;
#endif
        target = Point3f(yaw_offset, pitch_offset, distance);
}

float RuneDetector::cal_R_dis()
{
	RotatedRect next_armor1;	
	next_armor1.center=my_R.center;
	next_armor1.angle=0;
	next_armor1.size.height=my_R.size.height;
	next_armor1.size.width=my_R.size.width;
	       
	static float half_camera_width = 320;
        static float half_camera_height = 280;
        static float camera_fx = 1289.1;//cameraParam.fx
        static float camera_fy = 1228.7;//cameraParam.fy
        constexpr double rad = 57.295779513082320876798154814105;

        Point2f armor[4];
        next_armor1.points(armor);

        Point2f armor_up = (armor[0] + armor[1]) / 2;
        Point2f armor_down = (armor[2] + armor[3]) / 2;
        float armor_height = sqrt(powf(armor_down.x - armor_up.x, 2) + powf(armor_down.y - armor_up.y, 2));
        Point2f armor_center = (armor_up + armor_down) / 2;

        float distance = camera_fx * 125 / armor_height / 1000;
	return distance;
}

void RuneDetector::paint(Mat &src,Mat &dst1,bool &isnot,Point3f& target,int fps)
{
    if(!isnot||next_armor.center==Point2f(0,0))
    {
        putText(src, "No Armor!", Point(20, 50),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0,255, 0));
	putText(src, "fps: "+to_string(fps), Point(20, 150),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));
        imshow("grey",dst1);//名称要与下面的一致，否则会出现多个图，但实际上需要一个
    	imshow("show",src);
    	waitKey(1);
        return;
    }

    //描绘出装甲板，中心点，预测点
    Point centerP=armor_rect.center;

    if(mcu_data.enemy_color == EnemyColor::BLUE)
    {
        circle(src,centerP,1,Scalar(255,0,0),5);
        circle(src,next_armor.center,1,Scalar(0,255,0),5);
    }
    else
    {
        circle(src,centerP,1,Scalar(0,0,255),5);
        circle(src,next_armor.center,1,Scalar(0,255,0),5);
    }    
    Point2f Pnt[4],Pnt_next[4];
    armor_rect.points(Pnt);
    next_armor.points(Pnt_next);
    for(int j=0;j<4;++j)
    {
	    if(mcu_data.enemy_color == EnemyColor::BLUE)
	    {
		line(src,Pnt[j],Pnt[(j+1)%4],Scalar(255,0,0),4);
		line(src,Pnt_next[j],Pnt_next[(j+1)%4],Scalar(0,255,0),4);
	    }
	    else
	    {
		line(src,Pnt[j],Pnt[(j+1)%4],Scalar(0,0,255),4);
		line(src,Pnt_next[j],Pnt_next[(j+1)%4],Scalar(0,255,0),4);
	    }
    }
    putText(src, "Get Armor!", Point(20, 50),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0,255, 0));
    putText(src, "Distance:  "+to_string(target.z)+" meter", Point(20, 100),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0,255 , 0));
    putText(src, "yaw     :  "+to_string(target.x)+" degree", Point(20, 150),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0,255, 0));
    putText(src, "pitch   :  "+to_string(target.y)+" degree", Point(20, 200),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));

    if(mutex==small_rune)
        putText(src, "Rune :small", Point(20, 250),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));
    else if(mutex==big_rune)
        putText(src, "Rune :  big", Point(20, 250),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));
    else;
    putText(src, "fps: "+to_string(fps), Point(20, 300),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));

    if(is_get_R) 
    {
	putText(src, "Get R icon!", Point(20, 350),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));
	float R_dis=cal_R_dis();
	putText(src, "R_dis: "+to_string(R_dis), Point(20, 400),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));
    }
    else putText(src, "Not Get R!", Point(20, 350),CV_FONT_HERSHEY_PLAIN, 2, Scalar(0, 255, 0));
    
    imshow("grey",dst1);
    imshow("show",src);
    waitKey(1);
}

